function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

% SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)
%
%   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)
%
%   This filter assumes the following standard state-space model:
%
%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
%     y(k) = hfun[x(k),n(k),U2(k)]
%
%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
%   system.
%
%   INPUT
%         state                state mean at time k-1          ( xh(k-1) )
%         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(k-1) )
%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
%         InferenceDS          SPOK inference data structure generated by GENINFDS function.
%
%   OUTPUT
%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
%         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )
%         pNoise               process noise data structure     (possibly updated)
%         oNoise               observation noise data structure (possibly updated)
%
%         InternalVariablesDS  (optional) internal variables data structure
%            .xh_                 predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
%            .Sx_                 predicted state covariance (Cholesky factor)
%            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )
%            .inov                inovation signal
%            .Pinov               inovation covariance
%            .KG                  Kalman gain
%
%   Required InferenceDS fields:
%         .spkfParams           SPKF parameters = [h] with
%                                    h  :  CDKF scale factor / difference step size
%
%   See also
%   KF, EKF, UKF, CDKF, SRUKF
%   Copyright (c) Oregon Health & Science University (2006)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained from
%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
%   software should contact rebel@csee.ogi.edu for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

%=============================================================================================

Xdim  = InferenceDS.statedim;                                % extract state dimension
Odim  = InferenceDS.obsdim;                                  % extract observation dimension
U1dim = InferenceDS.U1dim;                                   % extract exogenous input 1 dimension
U2dim = InferenceDS.U2dim;                                   % extract exogenous input 2 dimension
Vdim  = InferenceDS.Vdim;                                    % extract process noise dimension
Ndim  = InferenceDS.Ndim;                                    % extract observation noise dimension

NOV = size(obs,2);                                           % number of input vectors

%------------------------------------------------------------------------------------------------------------------
%-- ERROR CHECKING

if (nargin ~= 8) error(' [ srcdkf ] Not enough input arguments.'); end

if (Xdim~=size(state,1)) error('[ srcdkf ] Prior state dimension differs from InferenceDS.statedim'); end
if (Xdim~=size(Sstate,1)) error('[ srcdkf ] Prior state covariance dimension differs from InferenceDS.statedim'); end
if (Odim~=size(obs,1)) error('[ srcdkf ] Observation dimension differs from InferenceDS.obsdim'); end
if U1dim
  [dim,nop] = size(U1);
  if (U1dim~=dim) error('[ srcdkf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'); end
  if (dim & (NOV~=nop)) error('[ srcdkf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'); end
end
if U2dim
  [dim,nop] = size(U2);
  if (U2dim~=dim) error('[ srcdkf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'); end
  if (dim & (NOV~=nop)) error('[ srcdkf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'); end
end

%------------------------------------------------------------------------------------------------------------------------

% setup buffer
xh   = zeros(Xdim,NOV);
xh_  = zeros(Xdim,NOV);
yh_  = zeros(Odim,NOV);
inov = zeros(Odim,NOV);

% Get index vectors for any of the state or observation vector components that are angular quantities
% which have discontinuities at +- Pi radians ?

sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
oA_IdxVec = InferenceDS.obsAngleCompIdxVec;


% Get and calculate CDKF scaling parameters and sigma point weights
h = InferenceDS.spkfParams(1);
hh = h^2;

W1 = [(hh - Xdim - Vdim)/hh   1/(2*hh);                  % sigma-point weights set 1
      1/(2*h)                sqrt(hh-1)/(2*hh)];

W2      = W1;
W2(1,1) = (hh - Xdim - Ndim)/hh ;                        % sigma-point weights set 2



switch InferenceDS.inftype

%======================================= PARAMETER ESTIMATION VERSION ===========================================
case 'parameter'

    Zeros_Xdim_X_Ndim     = zeros(Xdim,Ndim);
    Zeros_Ndim_X_Xdim     = zeros(Ndim,Xdim);

    nsp2 = 2*(Xdim+Ndim) + 1;                                % number of sigma points

    Sv = pNoise.cov;                             % Cholesky factor of process noise covariance
    dv = diag(Sv);
    Sn = oNoise.cov;                             % Cholesky factor of measurement noise covariance
    mu_n = oNoise.mu;

    Sx = Sstate;

    %---  Loop over all input vectors ---
    for i=1:NOV,

        UU2 = cvecrep(U2(:,i),nsp2);

        %------------------------------------------------------
        % TIME UPDATE

        xh_(:,i) = state;

        if pNoise.adaptMethod
        %--------------------------------------------
            switch pNoise.adaptMethod

            case 'lambda-decay'
                Sx_ = sqrt(pNoise.adaptParams(1))*Sx;

            case {'anneal','robbins-monro'}
                Sx_ = Sx + Sv;

            end
        %---------------------------------------------
        else
            Sx_ = Sx;
        end

        Z   = cvecrep([xh_(:,i); mu_n],nsp2);
        Sz  = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
        hSz  = h*Sz;
        Z(:,2:nsp2) = Z(:,2:nsp2) + [hSz -hSz];

        Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);

        %-- Calculate predicted observation mean, dealing with angular discontinuities if needed
        if isempty(oA_IdxVec)
            yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           % pediction of observation
            C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
            D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
        else
            obs_pivotA = Y_(oA_IdxVec,1);      % extract pivot angle
            Y_(oA_IdxVec,1) = 0;
            Y_(oA_IdxVec,2:nsp2) = subangle(Y_(oA_IdxVec,2:nsp2),cvecrep(obs_pivotA,nsp2-1));  % subtract pivot angle mod 2pi
            yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           % pediction of observation
            yh_(oA_IdxVec,i) = 0;
            for k=2:nsp2,
               yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   % calculate CDKF mean ... mod 2pi
            end
            C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
            D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
            C(oA_IdxVec,:) = subangle(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
            D(oA_IdxVec,:) = addangle(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec,Xdim+Ndim+2:nsp2));
            % Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted
            yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), obs_pivotA);  % add pivot angle back to calculate actual predicted mean
        end

        [temp,Sy] = qr([C D]',0);             % calculate inovation covariance using QR
        Sy = Sy';                             % convert back to ReBEL format

        if (Sy==0), Sy = eps; end

        Syx1 = C(:,1:Xdim);
        Syw1 = C(:,Xdim+1:end);

        Pxy = Sx_*Syx1';

        KG = (Pxy / Sy') / Sy;                % Kalman gain using LS solution with pivot


        if isempty(InferenceDS.innovation)
            inov(:,i) = obs(:,i) - yh_(:,i);
        else
            inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  % inovation (observation error)
        end


        if isempty(sA_IdxVec)
           xh(:,i) = xh_(:,i) + KG*inov(:,i);
        else
           upd = KG*inov(:,i);
           xh(:,i) = xh_(:,i) + upd;
           xh(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), upd(sA_IdxVec));
        end



        state = xh(:,i);

        Sx_ = Sx_';                           % ReBEL form -> Matlab form

        cov_update_vectors = KG*Sy;           % Correct covariance. This is equivalent to :  Px = Px_ - KG*Py*KG';
        for j=1:Odim
            Sx_ = cholupdate(Sx_,cov_update_vectors(:,j),'-');
        end

        Sx = Sx_';                            % Matlab form -> ReBEL form
        state = xh(:,i);

        if pNoise.adaptMethod
        %--- update process noise if needed -----------------------
            switch pNoise.adaptMethod

            case 'anneal'
                dV = max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2));
                ds = diag(Sx);
                dv = -ds + sqrt(dV + ds.^2);
                Sv = diag(dv);

            case 'robbins-monro'
                nu = 1/pNoise.adaptParams(1);
                dV = (1-nu)*(dv.^2) + nu*diag(KG*(KG*inov*inov')');
                ds = diag(Sx);
                dv = -ds + sqrt(dV + ds.^2);
                Sv = diag(dv);
                pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));

            otherwise
                error(' [ srukf ]  Process noise update method not allowed.');

            end

            pNoise.cov = Sv;
        %-----------------------------------------------------------
        end

    end   %... loop over all input vectors



otherwise
%===================================== STATE & JOINT ESTIMATION VERSION =========================================

    Zeros_Xdim_X_Vdim = zeros(Xdim,Vdim);
    Zeros_Vdim_X_Xdim = zeros(Vdim,Xdim);
    Zeros_Xdim_X_Ndim = zeros(Xdim,Ndim);
    Zeros_Ndim_X_Xdim = zeros(Ndim,Xdim);

    nsp1   = 2*(Xdim+Vdim) + 1;          % number of sigma points (first set)
    nsp2   = 2*(Xdim+Ndim) + 1;          % number of sigma points (second set)

    Sv = pNoise.cov;         % matrix square root of process noise covariance
    Sn = oNoise.cov;         % matrix square root of measurement noise covariance
    mu_v = pNoise.mu;      % get process noise mean
    mu_n = oNoise.mu;      % get measurement noise mean
    Sx = Sstate;                         % matrix square root of state covariance

    if (U1dim==0), UU1 = zeros(0,nsp1); end
    if (U2dim==0), UU2 = zeros(0,nsp2); end

    % if process noise adaptation for joint estimation
    if pNoise.adaptMethod
        switch InferenceDS.inftype
          case 'joint'
            idx = pNoise.idxArr(end,:);     % get indeces of parameter block of combo-gaussian noise source
            ind1 = idx(1);                  % beginning index of parameter section
            ind2 = idx(2);                  % ending index of parameter section
            paramdim = ind2-ind1+1;         % infer parameter vector length
            dv = diag(Sv);                  % grab diagonal
            dv = dv(ind1:ind2);             % extract the part of the diagonal that relates to the 'parameter section'
          case 'state'
            ind1 = 1;
            ind2 = Xdim;
            paramdim = Xdim;
            dv = diag(Sv);
        end
    end

    %--- Loop over all input vectors ---
    for i=1:NOV,

        if (U1dim), UU1 = cvecrep(U1(:,i),nsp1); end
        if (U2dim), UU2 = cvecrep(U2(:,i),nsp2); end

        %------------------------------------------------------
        % TIME UPDATE

        Z   = cvecrep([state; pNoise.mu],nsp1);
        Zm  = Z;                                                % copy needed for possible angle components section
        Sz  = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
        hSz = h*Sz;
        hSzM = [hSz -hSz];
        Z(:,2:nsp1) = Z(:,2:nsp1) + hSzM;         % build sigma-point set

        %-- Calculate predicted state mean, dealing with angular discontinuities if needed
        if isempty(sA_IdxVec)
            X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  % propagate sigma-points through process model
            xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
            A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
            B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - cvecrep(2*X_(:,1),Xdim+Vdim));
        else
            Z(sA_IdxVec,2:nsp1) = addangle(Zm(sA_IdxVec,2:nsp1), hSzM(sA_IdxVec,:));      % fix sigma-point set for angular components
            X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); % propagate sigma-points through process model
            state_pivotA = X_(sA_IdxVec,1);                                % extract pivot angle
            X_(sA_IdxVec,1) = 0;
            X_(sA_IdxVec,2:end) = subangle(X_(sA_IdxVec,2:end), cvecrep(state_pivotA,nsp1-1));  % subtract pivot angle mod 2pi
            xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
            xh_(sA_IdxVec,i) = 0;
            for k=2:nsp1,
                xh_(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), W1(1,2)*X_(sA_IdxVec,k));     % calculate CDKF mean ... mod 2pi
            end
            A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
            B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - cvecrep(2*X_(:,1),Xdim+Vdim));
            A(sA_IdxVec,:) = subangle(W1(2,1)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,1)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
            B(sA_IdxVec,:) = addangle(W1(2,2)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,2)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
            % Note for above line : Remember, X_(sA_IdxVec,1) = 0, so the last term of B expression need not be subtracted
            xh_(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), state_pivotA);  % add pivot angle back to calculate actual predicted mean
        end

        [temp,Sx_] = qr([A B]',0);
        Sx_= Sx_';


        Z  = cvecrep([xh_(:,i); oNoise.mu] ,nsp2);
        Zm = Z;                                                 % copy needed for possible angle components section
        Sz = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
        hSz = h*Sz;
        hSzM = [hSz -hSz];
        Z(:,2:nsp2) = Z(:,2:nsp2) + hSzM;

        %-- Calculate predicted observation mean, dealing with angular discontinuities if needed
        if isempty(oA_IdxVec)
            Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
            yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);
            C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
            D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
        else
            Z(oA_IdxVec,2:nsp2) = addangle(Zm(oA_IdxVec,2:nsp2), hSzM(oA_IdxVec,:));  % fix sigma-point set for angular components
            Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
            obs_pivotA = Y_(oA_IdxVec,1);      % extract pivot angle
            Y_(oA_IdxVec,1) = 0;
            Y_(oA_IdxVec,2:nsp2) = subangle(Y_(oA_IdxVec,2:nsp2),cvecrep(obs_pivotA,nsp2-1));  % subtract pivot angle mod 2pi
            yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           % pediction of observation
            yh_(oA_IdxVec,i) = 0;
            for k=2:nsp2,
               yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   % calculate CDKF mean ... mod 2pi
            end
            C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
            D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - cvecrep(2*Y_(:,1),Xdim+Ndim));
            C(oA_IdxVec,:) = subangle(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
            D(oA_IdxVec,:) = addangle(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec,Xdim+Ndim+2:nsp2));
            % Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted
            yh_(oA_IdxVec,i) = addangle(yh_(oA_IdxVec,i), obs_pivotA);  % add pivot angle back to calculate actual predicted mean
        end

        [temp,Sy] = qr([C D]',0);
        Sy = Sy';


        %------------------------------------------------------
        % MEASUREMENT UPDATE

        Syx1 = C(:,1:Xdim);
        Syw1 = C(:,Xdim+1:end);

        Pxy = Sx_*Syx1';

        KG = (Pxy / Sy') / Sy;

        if isempty(InferenceDS.innovation)
            inov(:,i) = obs(:,i) - yh_(:,i);
            if ~isempty(oA_IdxVec)
              inov(oA_IdxVec,i) = subangle(obs(oA_IdxVec,i), yh_(oA_IdxVec,i));
            end
        else
            inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  % inovation (observation error)
        end

        if isempty(sA_IdxVec)
           xh(:,i) = xh_(:,i) + KG*inov(:,i);
        else
           upd = KG*inov(:,i);
           xh(:,i) = xh_(:,i) + upd;
           xh(sA_IdxVec,i) = addangle(xh_(sA_IdxVec,i), upd(sA_IdxVec));
        end

        state = xh(:,i);

        [temp,Sx] = qr([Sx_-KG*Syx1 KG*Syw1 KG*D]',0);
        Sx=Sx';


        if pNoise.adaptMethod
        %--- update process noise if needed for joint estimation ----------------------
            switch pNoise.adaptMethod

            case 'anneal'
                dv = sqrt(max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2)));
                Sv(ind1:ind2,ind1:ind2) = diag(dv);

             case 'robbins-monro'
                nu = 1/pNoise.adaptParams(1);
                subKG = KG(end-paramdim+1:end,:);
                dv = sqrt((1-nu)*(dv.^2) + nu*diag(subKG*(subKG*inov*inov')'));
                Sv(ind1:ind2,ind1:ind2) = diag(dv);
                pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));

            otherwise
                error(' [ srcdkf ]  Process noise update method not allowed.');

            end

            pNoise.cov = Sv;


        %-----------------------------------------------------------
        end

    end   %--- loop over all input vectors

%====================================================================================================================
end


if (nargout>4),
    InternalVariablesDS.xh_   = xh_;
    InternalVariablesDS.Sx_   = Sx_;
    InternalVariablesDS.yh_   = yh_;
    InternalVariablesDS.inov  = inov;
    InternalVariablesDS.Sinov = Sy;
    InternalVariablesDS.KG    = KG;
end




